#include "GeometryHelper.hpp"
#include <Resource\Mesh\Mesh.hpp>

namespace zzz{
void SphereSubDivide(SimpleMesh &mesh)
{
  unsigned int origSize = mesh.faces_.size();
  for (unsigned int i = 0 ; i < origSize ; ++i)
  {
    Vector3i &t = mesh.faces_[i];
    const Vector3f &a = mesh.vertices_[t[0]], &b = mesh.vertices_[t[1]], &c = mesh.vertices_[t[2]];
    Vector3f v1(a+b);
    Vector3f v2(a+c);
    Vector3f v3(b+c);
    v1.Normalize();
    v2.Normalize();
    v3.Normalize();
    int ia=t[0], ib=t[1], ic=t[2], iv1, iv2, iv3;
    vector<Vector3f>::iterator vi1=find(mesh.vertices_.begin(),mesh.vertices_.end(),v1);
    if (vi1==mesh.vertices_.end())
    {
      mesh.vertices_.push_back(v1);
      iv1=mesh.vertices_.size()-1;
    }
    else iv1=vi1-mesh.vertices_.begin();
    vector<Vector3f>::iterator vi2=find(mesh.vertices_.begin(),mesh.vertices_.end(),v2);
    if (vi2==mesh.vertices_.end())
    {
      mesh.vertices_.push_back(v2);
      iv2=mesh.vertices_.size()-1;
    }
    else iv2=vi2-mesh.vertices_.begin();
    vector<Vector3f>::iterator vi3=find(mesh.vertices_.begin(),mesh.vertices_.end(),v3);
    if (vi3==mesh.vertices_.end())
    {
      mesh.vertices_.push_back(v3);
      iv3=mesh.vertices_.size()-1;
    }
    else iv3=vi3-mesh.vertices_.begin();
    t[0] = iv1; t[1] = iv3; t[2] = iv2; // overwrite the original
    mesh.faces_.push_back(Vector3i(ia, iv1, iv2));
    mesh.faces_.push_back(Vector3i(ic, iv2, iv3));
    mesh.faces_.push_back(Vector3i(ib, iv3, iv1));
  }
}

void GeometryHelper::CreateSphere(SimpleMesh &mesh, int levels)
{
    mesh.vertices_.clear();
    mesh.faces_.clear();

    // build an icosahedron
    float t = (1 + sqrt(5.0))/2.0;
    float s = sqrt(1 + t*t);
    // create the 12 vertices
    mesh.vertices_.push_back(Vector3f(t, 1, 0)/s);
    mesh.vertices_.push_back(Vector3f(-t, 1, 0)/s);
    mesh.vertices_.push_back(Vector3f(t, -1, 0)/s);
    mesh.vertices_.push_back(Vector3f(-t, -1, 0)/s);
    mesh.vertices_.push_back(Vector3f(1, 0, t)/s);
    mesh.vertices_.push_back(Vector3f(1, 0, -t)/s);
    mesh.vertices_.push_back(Vector3f(-1, 0, t)/s);
    mesh.vertices_.push_back(Vector3f(-1, 0, -t)/s);
    mesh.vertices_.push_back(Vector3f(0, t, 1)/s);
    mesh.vertices_.push_back(Vector3f(0, -t, 1)/s);
    mesh.vertices_.push_back(Vector3f(0, t, -1)/s);
    mesh.vertices_.push_back(Vector3f(0, -t, -1)/s);

    // create the 20 triangles_
    mesh.faces_.push_back(Vector3i(0, 8, 4));
    mesh.faces_.push_back(Vector3i(1, 10, 7));
    mesh.faces_.push_back(Vector3i(2, 9, 11));
    mesh.faces_.push_back(Vector3i(7, 3, 1));    
    mesh.faces_.push_back(Vector3i(0, 5, 10));
    mesh.faces_.push_back(Vector3i(3, 9, 6));
    mesh.faces_.push_back(Vector3i(3, 11, 9));
    mesh.faces_.push_back(Vector3i(8, 6, 4));    
    mesh.faces_.push_back(Vector3i(2, 4, 9));
    mesh.faces_.push_back(Vector3i(3, 7, 11));
    mesh.faces_.push_back(Vector3i(4, 2, 0));
    mesh.faces_.push_back(Vector3i(9, 4, 6));    
    mesh.faces_.push_back(Vector3i(2, 11, 5));
    mesh.faces_.push_back(Vector3i(0, 10, 8));
    mesh.faces_.push_back(Vector3i(5, 0, 2));
    mesh.faces_.push_back(Vector3i(10, 5, 7));    
    mesh.faces_.push_back(Vector3i(1, 6, 8));
    mesh.faces_.push_back(Vector3i(1, 8, 10));
    mesh.faces_.push_back(Vector3i(6, 1, 3));
    mesh.faces_.push_back(Vector3i(11, 7, 5));

    for (int ctr = 0; ctr < levels; ctr++) SphereSubDivide(mesh);
}

double GeometryHelper::AngleFromSinCos(double sinalpha, double cosalpha)
{
  sinalpha=Clamp<double>(-1,sinalpha,1);
  cosalpha=Clamp<double>(-1,cosalpha,1);
  if (sinalpha > 0)  // phase 1, 2
    return acos(cosalpha);
  return C_2PI - acos(cosalpha);
}

Matrix3x3f GeometryHelper::GenTexCoordFlat(Mesh<3> &mesh, zuint group, double pixel_per_one, zuint &u_size, zuint &v_size)
{
  ZCHECK(group<mesh.groups_.size());
  TriMesh::MeshGroup *g=mesh.groups_[group];

  vector<int> used_points(mesh.pos_.size(), -1);
  for (zuint i=0; i<g->facep_.size(); i++) {
    used_points[g->facep_[i][0]]=1;
    used_points[g->facep_[i][1]]=1;
    used_points[g->facep_[i][2]]=1;
  }

  vector<Vector3f> selected;
  for (zuint i=0; i<used_points.size(); i++) {
    if (used_points[i]==1)
      selected.push_back(mesh.pos_[i]);
  }

  // project to 2d plane
  Matrix3x3f evector;
  Vector3f evalue;
  PCA<3,float>(selected, evector, evalue);

  for (zuint i=0; i<selected.size(); i++)
    selected[i]=evector * selected[i];

  // calculate projected size
  AABB<3,float> aabb;
  aabb+=selected;
  if (aabb.Diff(2)/aabb.Diff(0)>0.01 || aabb.Diff(2)/aabb.Diff(1)>0.01)
    ZLOGV<<"The mesh is not very flat, "<<ZVAR(aabb.Diff())<<endl;

  // decide tex size
  u_size = aabb.Diff(0)*pixel_per_one;
  v_size = aabb.Diff(1)*pixel_per_one;

  float u_scale = float(u_size-1)/u_size;
  float v_scale = float(v_size-1)/v_size;
  float u_offset = 0.5f / u_size;
  float v_offset = 0.5f / v_size;

  for (zuint i=0, j=0; i<used_points.size(); i++) {
    if (used_points[i]==1) {
      used_points[i]=mesh.tex_.size();
      const Vector3f &tex=selected[j++];
      mesh.tex_.push_back(Vector3f(
        (tex[0]-aabb.Min(0)) / aabb.Diff(0) * u_scale + u_offset,
        (tex[1]-aabb.Min(1)) / aabb.Diff(1) * v_scale + v_offset,
        0));
    }
  }

  g->facet_.clear();
  for (zuint i=0; i<g->facep_.size(); i++) {
    g->facet_.push_back(Vector3i(used_points[g->facep_[i][0]],
                                  used_points[g->facep_[i][1]],
                                  used_points[g->facep_[i][2]]));
  }

  mesh.SetFlag(MESH_TEX);
  g->SetFlag(MESH_TEX);
  return evector;
}

bool GeometryHelper::RasterizePosToImage(const Mesh<3> &mesh, zuint group, Array<2,Vector3f> &pos_img, Array2uc &pos_taken)
{
  pos_taken.SetSize(pos_img.Size());
  pos_taken.Zero();
  
  TriMesh::MeshGroup *g = mesh.groups_[group];
  int u_size = pos_img.Size(1);
  int v_size = pos_img.Size(0);
  ZCHECK(mesh.HasFlag(MESH_POS));
  ZCHECK(mesh.HasFlag(MESH_TEX));
  ZCHECK(g->HasFlag(MESH_POS));
  ZCHECK(g->HasFlag(MESH_TEX));
  for (zuint tri = 0; tri < g->facep_.size(); tri++) {
    const Vector2f tex_coord0(mesh.tex_[g->facet_[tri][0]]);
    const Vector2f tex_coord1(mesh.tex_[g->facet_[tri][1]]);
    const Vector2f tex_coord2(mesh.tex_[g->facet_[tri][2]]);
    const Vector3f &pos_coord0(mesh.pos_[g->facep_[tri][0]]);
    const Vector3f &pos_coord1(mesh.pos_[g->facep_[tri][1]]);
    const Vector3f &pos_coord2(mesh.pos_[g->facep_[tri][2]]);

    AABB<2, float> aabb;
    aabb += tex_coord0;
    aabb += tex_coord1;
    aabb += tex_coord2;
    int u_min = Clamp<int>(0, floor(aabb.Min(0) * (u_size-1)), u_size-1);
    int u_max = Clamp<int>(0, ceil(aabb.Max(0) * (u_size-1)), u_size-1);
    int v_min = Clamp<int>(0, floor(aabb.Min(1) * (v_size-1)), v_size-1);
    int v_max = Clamp<int>(0, ceil(aabb.Max(1) * (v_size-1)), v_size-1);
    for (int v = v_min; v <= v_max; v++) for (int u = u_min; u <= u_max; u++) {
      // Generate point
      Vector2f uv_coord(float(u-0.5f/u_size)/u_size, float(v-0.5f/v_size)/v_size);
      if (!PointInTriangle2D(tex_coord0, tex_coord1, tex_coord2, uv_coord))
        continue;
      Vector3f bary_coord;
      Barycentric<float, float>(uv_coord, tex_coord0, tex_coord1, tex_coord2, bary_coord);
      pos_img(v, u) = pos_coord0 * bary_coord[0] + pos_coord1 * bary_coord[1] + pos_coord2 * bary_coord[2];
      pos_taken(v, u) = 1;
    }
  }
  return true;
}

}  // namespace zzz

